#include "communication.hpp"
#include "dummy_robot.h"
#include "user_define.h"

extern DummyRobot dummy;
extern DOF6Kinematic::Joint6D_t LastJointToTarDiffMin,JointToTarDiffFabs;

inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints, uint8_t &_index) {
    float max = -1;
    for (uint8_t i = 0; i < dummy.RobotAxisNum; i++) {
        if (abs(_joints.a[i]) > max) {
            max = abs(_joints.a[i]);
            _index = i;
        }
    }

    return max;
}

inline float AbsSumMaxOf6(DOF6Kinematic::Joint6D_t _joints, uint8_t &_index) {
    float sum = 0;
    const float F[6] = {1,1,1,360,1,1};
    for (uint8_t i = 0; i < dummy.RobotAxisNum; i++) {
        sum += abs(_joints.a[i])*F[i];
    }

    return sum;
}


DummyRobot::DummyRobot(CAN_HandleTypeDef *_hcan) :
        hcan(_hcan) {

#ifdef DUMMY_T

    //第二版本
    /*motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
    motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 50, -180, 180);
    motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 50, -73, 120);
    motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 50, 35, 180);
    motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 50, -180, 180);
    motorJ[5] = new CtrlStepMotor(_hcan, 5, false, 30, -120, 120);
    motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -300, 300);
    motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
     */

    //第三版本
    motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
    motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 50, -175, 174);
    motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 100, -72, 120);
    motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 50, 35, 180);
    motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 50, -180, 180);
    motorJ[5] = new CtrlStepMotor(_hcan, 5, false, 50, -120, 90);
    motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -300, 300);
    motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);

#endif
#ifdef AR3_T
    motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
    motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 40, -148, 164);
    motorJ[2] = new CtrlStepMotor(_hcan, 2, true, 50, -48     , 120);
    motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 50, -134, 152);
    motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 39.2, -146, 47);
    motorJ[5] = new CtrlStepMotor(_hcan, 5, false, 10.5, -110, 110);
    motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 20, -200, 160);
    motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
#endif
#ifdef SITO_T
    motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
    motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 101, -720, 720);
    motorJ[2] = new CtrlStepMotor(_hcan, 2, true, 101, -180, 180);
    motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 51, -180, 180);
    motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 51, -720, 720);
    motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 51, -180, 180);
    motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 51, -720, 720);
    motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
#endif
#ifdef BATTLE_CHARGE_THERE_AXIS_T
    #if HARD_VER_ROBOT_NUM == 0
    motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 4800, -1.5, 1.5);
    motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 4788, -0.08, 1.38);
    motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 9605, -0.02, 1.45);
    motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 4800, -0.18, 0.02);
    motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 4804, -0.08, 1.38);
    motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 1, 0, 0);
    motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 1, 0, 0);
    motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
    #if HARD_VER_ROBOT_NUM == 1
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 4800, -1.5, 1.5);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 4800, -0.14, 1.47);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 9605, -0.06, 1.39);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 50, -300, 300);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, false, 4800, -0.14, 1.47);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 1, -1000, 1000);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 1, -1000, 1000);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
#endif
#if defined(IMU6_T)|| defined(IRC_T)
    #if HARD_VER_ROBOT_NUM == 0
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 4800, -1.5, 1.5);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 4788, -0.08, 1.38);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 9605, -0.02, 1.45);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 4800, -0.18, 0.02);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 4804, -0.08, 1.38);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 1, 0, 0);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 1, 0, 0);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
    #if HARD_VER_ROBOT_NUM == 1
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -1.5, 1.5);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 5000, -0.14, 1.47);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, true, 5000, -0.06, 1.39);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 5000, -0.175, 0.03);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, false, 1, -1000, 1000);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 1, -1000, 1000);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 1, -1000, 1000);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
#endif
#ifdef MR6_T
    #if HARD_VER_ROBOT_NUM == 1
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 200, -180, 180);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 200, -29     , 120);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 150, -8, 155);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 30, -360, 360);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 30, -117, 117);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -200, 160);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
    #if HARD_VER_ROBOT_NUM == 2
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 200, -180, 180);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 200, -29     , 120);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 150, -8, 155);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 35, -360, 360);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 35, -117, 117);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -360*1.5, 360*1.5);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -300, 300);
    #endif
    #if HARD_VER_ROBOT_NUM == 3
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 200, -360*2, 360*2);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 200, -60     , 120);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 150, -8, 155);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 32, -360, 360);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 32, -117, 117);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -360*1.5, 360*1.5);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 1, -300, 300);
    #endif
    #if HARD_VER_ROBOT_NUM == 4
        motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
        motorJ[1] = new CtrlStepMotor(_hcan, 1, false, 200, -360*2, 360*2);
        motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 200, -36     , 120);
        motorJ[3] = new CtrlStepMotor(_hcan, 3, false, 150, -8, 155);
        motorJ[4] = new CtrlStepMotor(_hcan, 4, true, 30, -360, 360);
        motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 30, -117, 117);
        motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -400, 400);
        motorJ[7] = new CtrlStepMotor(_hcan, 7, true, 50, -360, 300);
    #endif

#endif
    hand = new DummyHand(_hcan, 7);

#ifdef DUMMY_T
    dof6Solver = new DOF6Kinematic(0.168f, 0.035f, 0.150f, 0.115f, 0.055f, 0.270f);
#endif
#ifdef MR6_T
    dof6Solver = new DOF6Kinematic(0.260f, -0.08116f, 0.31206f,  0.38638f, 0.08494f, 0.135f);
#endif

}


DummyRobot::~DummyRobot() {
    for (int j = 0; j <= 6; j++)
        delete motorJ[j];

    delete hand;
    delete dof6Solver;
}


void DummyRobot::Init() {
    SetCommandMode(DEFAULT_COMMAND_MODE);
    SetJointSpeed(DEFAULT_JOINT_SPEED);
}


void DummyRobot::Reboot() {
    motorJ[ALL]->Reboot();
    osDelay(500); // waiting for all joints done
    HAL_NVIC_SystemReset();
}


void DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints) {
    for (int j = 1; j <= 6; j++) {
        motorJ[j]->SetAngleWithVelocityLimit(_joints.a[j - 1] - initPose.a[j - 1],
                                             dynamicJointSpeeds.a[j - 1]);
    }
}

bool DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6) {
    DOF6Kinematic::Joint6D_t targetJointsTmp(_j1, _j2, _j3, _j4, _j5, _j6);
    bool valid = true;

    for (int j = 1; j <= dummy.RobotAxisNum; j++) {
        if (targetJointsTmp.a[j - 1] > motorJ[j]->angleLimitMax ||
            targetJointsTmp.a[j - 1] < motorJ[j]->angleLimitMin)
            valid = false;
    }

    if (valid) {
        DOF6Kinematic::Joint6D_t deltaJoints = targetJointsTmp - currentJoints;
        for (int i = 0; i < 6; ++i) {
            if(deltaJoints.a[i] > 0)
                IsTarAdd[i] = true;
            else
                IsTarAdd[i] = false;

        }
        uint8_t index;
        float maxAngle = AbsMaxOf6(deltaJoints, index);
       // printf("err %.3f %.3f \n",targetJointsTmp.a[index],currentJoints.a[index]);
        //float time = maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed;
        if(fabs(maxAngle) < 0.000001)
            return true;

        float time = maxAngle * (float) (2*PI*10) / jointSpeed;
        //Respond(*usbStreamOutputPtr,"\njointSpeed:%.3f MaxAngle(%d):%.3f time:%.3f \n",jointSpeed,index,maxAngle,time);
        for (int j = 1; j <= dummy.RobotAxisNum; j++) {
            dynamicJointSpeeds.a[j - 1] =
                    abs(deltaJoints.a[j - 1] * (float) (motorJ[j]->reduction) / time * 0.1f); //0~10r/s
        }

        jointsStateFlag = 0;
        targetJoints = targetJointsTmp;

        dof6Solver->SolveFK(targetJoints, dummy.targetPose6D);
        dummy.targetPose6D.X *= 1000; // m -> mm
        dummy.targetPose6D.Y *= 1000; // m -> mm
        dummy.targetPose6D.Z *= 1000; // m -> mm


        RunToTarTimesMs = 0;
//        RunToTarAllTimesMs = (maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed)*1000;
//        printf("\njointSpeed:%.3f MaxAngle(%d):%.3f time:%d \n",jointSpeed,index,maxAngle,RunToTarAllTimesMs);
        return true;
    }

    return false;
}


bool DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c) {
    DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c);
    DOF6Kinematic::IKSolves_t ikSolves{};
    DOF6Kinematic::Joint6D_t lastJoint6D{};

    dof6Solver->SolveIK(pose6D, lastJoint6D, ikSolves);

    bool valid[8];
    int validCnt = 0;

    for (int i = 0; i < 8; i++) {
        valid[i] = true;

        for (int j = 0; j < dummy.RobotAxisNum; ++j) {
            if (ikSolves.config[i].a[j] - dummy.currentJoints.a[j] > 180){
                ikSolves.config[i].a[j] = ikSolves.config[i].a[j] - 360;
            }else if (ikSolves.config[i].a[j] - dummy.currentJoints.a[j] < -180){
                ikSolves.config[i].a[j] = ikSolves.config[i].a[j] + 360;
            }
        }
        for (int j = 0; j < dummy.RobotAxisNum; j++) {
            if (ikSolves.config[i].a[j] > motorJ[j+1]->angleLimitMax ||
                ikSolves.config[i].a[j] < motorJ[j+1]->angleLimitMin) {
                valid[i] = false;
                continue;
            }
        }

        if (valid[i]) validCnt++;
    }

    if (validCnt) {
        float min = 1000000000;
        uint8_t indexConfig = 0, indexJoint = 0;
        for (int i = 0; i < 8; i++) {
            if (valid[i]) {
                //printf("PJ%d,>%.1f,%.1f,%.1f,%.1f,%.1f,%.1f\n",i,ikSolves.config[i].a[0],ikSolves.config[i].a[1],ikSolves.config[i].a[2],ikSolves.config[i].a[3],ikSolves.config[i].a[4],ikSolves.config[i].a[5]);
                for (int j = 0; j < dummy.RobotAxisNum; j++)
                    lastJoint6D.a[j] = ikSolves.config[i].a[j];
                DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D;

//                for (int j =0; j < 6; ++j) {
//                    if (tmp.a[j] > 180){
//                        tmp.a[j] = tmp.a[j] - 360;
//                    }else if (tmp.a[i] < -180){
//                        tmp.a[j] = tmp.a[j] + 360;
//                    }
//                }
//                for (int j = 0; j < 6; ++j) {
//                    ikSolves.config[i].a[j] += tmp.a[j];
//                }

                float maxAngle = AbsSumMaxOf6(tmp, indexJoint);
                if (maxAngle < min) {
                    min = maxAngle;
                    indexConfig = i;
                }
                //printf("P%d,>%.1f,%.1f,%.1f,%.1f,%.1f,%.1f Su:%.1f se:%d\n",i,tmp.a[0],tmp.a[1],tmp.a[2],tmp.a[3],tmp.a[4],tmp.a[5],maxAngle,indexConfig);
            }
            //if(i == 7)
              //  printf("\n\n");
        }
//        if(PUtFullBatProcessStep > 7){
//            if(indexConfig == 0 && valid[1] && fabs(ikSolves.config[1].a[3]) > fabs(ikSolves.config[0].a[3]))
//                indexConfig = 1;
//            else if(indexConfig == 1 && valid[0] && fabs(ikSolves.config[0].a[3]) > fabs(ikSolves.config[1].a[3]))
//                indexConfig = 0;
//        }


        return MoveJ(ikSolves.config[indexConfig].a[0], ikSolves.config[indexConfig].a[1],
                     ikSolves.config[indexConfig].a[2], ikSolves.config[indexConfig].a[3],
                     ikSolves.config[indexConfig].a[4], ikSolves.config[indexConfig].a[5]);
    }

    return false;
}

void DummyRobot::UpdateJointAngles() {
    //motorJ[ALL]->UpdateAngle();
    for (int i = 1; i <= dummy.RobotAxisNum ; ++i) {
        motorJ[i]->UpdateAngle();
    }
}


void DummyRobot::UpdateJointAnglesCallback() {
    for (int i = 1; i <= 6; i++) {
        currentJoints.a[i - 1] = motorJ[i]->angle + initPose.a[i - 1];
#ifndef IMU6_T
        if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS){
            //currentJoints.a[i - 1] = currentJoints.a[i - 1] / motorJ[i]->reduction;
        }
#else
        if(dummy.HarVer == IMU6){
            //currentJoints.a[i - 1] = currentJoints.a[i - 1] / motorJ[i]->reduction;
        }
#endif
        if (motorJ[i]->state == CtrlStepMotor::FINISH)
            jointsStateFlag |= (1 << i);
        else
            jointsStateFlag &= ~(1 << i);
    }
}


void DummyRobot::SetJointSpeed(float _speed) {
    if (_speed < 0)_speed = 0;
    else if (_speed > 300) {
#ifndef IMU6_T
        if(dummy.HarVer != BATTLE_CHARGE_THERE_AXIS)
            _speed = 300;
#else
        if(dummy.HarVer != IMU6)
            _speed = 300;
#endif
        else if(_speed > 10000)
            _speed = 10000;
    }

    jointSpeed = _speed * jointSpeedRatio;
    SpeedLimit = _speed;
}


void DummyRobot::SetJointAcceleration(float _acc) {
    if (_acc < 0)_acc = 0;
    else if (_acc > 100) _acc = 100;

    for (int i = 1; i <= 6; i++)
        motorJ[i]->SetAcceleration(_acc / 100 * DEFAULT_JOINT_ACCELERATION_BASES.a[i - 1]);
}


void DummyRobot::CalibrateHomeOffset() {
    // Disable FixUpdate, but not disable motors
    isEnabled = false;
    motorJ[ALL]->SetEnable(true);

    // 1.Manually move joints to L-Pose [precisely]
    // ...
    motorJ[2]->SetCurrentLimit(0.5);
    motorJ[3]->SetCurrentLimit(0.5);
    osDelay(500);

    // 2.Apply Home-Offset the first time
    motorJ[ALL]->ApplyPositionAsHome();
    osDelay(500);

    // 3.Go to Resting-Pose
    initPose = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
    currentJoints = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
    Resting();
    osDelay(500);

    // 4.Apply Home-Offset the second time
    motorJ[ALL]->ApplyPositionAsHome();
    osDelay(500);
    motorJ[2]->SetCurrentLimit(1);
    motorJ[3]->SetCurrentLimit(1);
    osDelay(500);

    Reboot();
}


void DummyRobot::Homing() {
    float lastSpeed = jointSpeed;
    SetJointSpeed(10);

    MoveJ(0, 0, 90, 0, 0, 0);
    MoveJoints(targetJoints);
    while (IsMoving())
        osDelay(10);

    SetJointSpeed(lastSpeed);
}


void DummyRobot::Resting() {
    float lastSpeed = jointSpeed;
    SetJointSpeed(10);

    MoveJ(REST_POSE.a[0], REST_POSE.a[1], REST_POSE.a[2],
          REST_POSE.a[3], REST_POSE.a[4], REST_POSE.a[5]);
    MoveJoints(targetJoints);
    while (IsMoving())
        osDelay(10);

    SetJointSpeed(lastSpeed);
}


void DummyRobot::SetEnable(bool _enable) {
    if(dummy.HarVer != SITO){
        motorJ[ALL]->SetEnable(_enable);
    }
    isEnabled = _enable;
}


void DummyRobot::UpdateJointPose6D() {
    if(dummy.RobotAxisNum <= 4){
        currentPose6D.X = currentJoints.a[1]; // m
        currentPose6D.Y = currentJoints.a[0]; // m
        currentPose6D.Z = currentJoints.a[2]; // m
    }else{
        dof6Solver->SolveFK(currentJoints, currentPose6D);
        currentPose6D.X *= 1000; // m -> mm
        currentPose6D.Y *= 1000; // m -> mm
        currentPose6D.Z *= 1000; // m -> mm
    }

}


bool DummyRobot::IsMoving() {
    if(dummy.RobotAxisNum == 3)
        return jointsStateFlag != 0b00001110;
    else if(dummy.RobotAxisNum == 4)
        return jointsStateFlag != 0b00011110;
    else if(dummy.RobotAxisNum == 5)
        return jointsStateFlag != 0b00111110;
    else
        return jointsStateFlag != 0b01111110;
}

bool DummyRobot::IsAllJointsEnable() {
    return jointsEnableFlag == 0b01111110;
}

bool DummyRobot::IsArrived(float err) {
    for (int i = 0; i < dummy.RobotAxisNum; i++) {
        if(fabs(dummy.currentJoints.a[i]-dummy.targetJoints.a[i]) > err)
            return false;
    }
    return true;
}

bool DummyRobot::IsPosArrived(float XErrPermit,float YErrPermit,float ZErrPermit,float AErrPermit,float BErrPermit,float CErrPermit) {
    if(fabs(
            dummy.currentPose6D.X - dummy.targetPose6D.X) < XErrPermit && fabs(dummy.currentPose6D.Y - dummy.targetPose6D.Y) < YErrPermit && fabs(dummy.currentPose6D.Z - dummy.targetPose6D.Z) < ZErrPermit &&
            (fabs(dummy.currentPose6D.A - dummy.targetPose6D.A) < AErrPermit || fabs(dummy.currentPose6D.A + dummy.targetPose6D.A) < AErrPermit) &&
            (fabs(dummy.currentPose6D.B - dummy.targetPose6D.B) < BErrPermit || fabs(dummy.currentPose6D.B + dummy.targetPose6D.B) < BErrPermit) &&
            (fabs(dummy.currentPose6D.C - dummy.targetPose6D.C) < CErrPermit || fabs(dummy.currentPose6D.C + dummy.targetPose6D.C) < CErrPermit)
            )
        return true;
    return false;
}

float DummyRobot::DisOfTwoPos(float Ax,float Ay,float Az,float Bx,float By,float Bz){
    return sqrt((Ax-Bx)*(Ax-Bx) + (Ay-By)*(Ay-By) + (Az-Bz)*(Az-Bz));
}


bool DummyRobot::IsEnabled() {
    return isEnabled;
}


void DummyRobot::SetCommandMode(uint32_t _mode) {
    if (_mode < COMMAND_TARGET_POINT_SEQUENTIAL ||
        _mode > COMMAND_MOTOR_TUNING)
        return;

    commandMode = static_cast<CommandMode>(_mode);

    switch (commandMode) {
        case COMMAND_TARGET_POINT_SEQUENTIAL:
        case COMMAND_TARGET_POINT_INTERRUPTABLE:
            jointSpeedRatio = 1;
            SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_LOW);
            break;
        case COMMAND_CONTINUES_TRAJECTORY:
            SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_HIGH);
            jointSpeedRatio = 0.3;
            break;
        case COMMAND_MOTOR_TUNING:
            break;
    }
}


DummyHand::DummyHand(CAN_HandleTypeDef *_hcan, uint8_t
_id) :
        nodeID(_id), hcan(_hcan) {
    txHeader =
            {
                    .StdId = 0,
                    .ExtId = 0,
                    .IDE = CAN_ID_STD,
                    .RTR = CAN_RTR_DATA,
                    .DLC = 8,
                    .TransmitGlobalTime = DISABLE
            };
}


void DummyHand::SetAngle(float _angle) {
    if (_angle > 300)_angle = 300;
    if (_angle < 0)_angle = 0;

    uint8_t mode = 0x02;
    txHeader.StdId = 7 << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_angle;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void DummyHand::SetMaxCurrent(float _val) {
    if (_val > 1)_val = 1;
    if (_val < 0)_val = 0;

    uint8_t mode = 0x01;
    txHeader.StdId = 7 << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void DummyHand::SetEnable(bool _enable) {
    if (_enable)
        SetMaxCurrent(maxCurrent);
    else
        SetMaxCurrent(0);
}


uint32_t DummyRobot::CommandHandler::Push(const std::string &_cmd) {
    osStatus_t status = osMessageQueuePut(commandFifo, _cmd.c_str(), 0U, 0U);
    if (status == osOK)
        return osMessageQueueGetSpace(commandFifo);

    return 0xFF; // failed
}


void DummyRobot::CommandHandler::EmergencyStop() {
    context->MoveJ(context->currentJoints.a[0], context->currentJoints.a[1], context->currentJoints.a[2],
                   context->currentJoints.a[3], context->currentJoints.a[4], context->currentJoints.a[5]);
    context->MoveJoints(context->targetJoints);
    context->isEnabled = false;
    ClearFifo();
}


std::string DummyRobot::CommandHandler::Pop(uint32_t timeout) {
    osStatus_t status = osMessageQueueGet(commandFifo, strBuffer, nullptr, timeout);

    return std::string{strBuffer};
}


uint32_t DummyRobot::CommandHandler::GetSpace() {
    return osMessageQueueGetSpace(commandFifo);
}


uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd) {
    uint8_t argNum;
	bool CommandRightFlag;

    if(_cmd[0] == 'M'){
        if(strstr(&_cmd[3],"[A]") != NULL) {
            int id;
            float Angle,speedLimit;
            argNum = sscanf(_cmd.c_str(), "M%d [A] %f %f", &id,&Angle,&speedLimit);
            Respond(*usbStreamOutputPtr, "Id:%d Angle:%.3f V:%.3f\n", id,Angle,speedLimit);

            if(id>0 && id<=6){
                dummy.targetJoints.a[id-1] = Angle;
                context->SetJointSpeed(speedLimit);
                CommandRightFlag = context->MoveJ(dummy.targetJoints.a[0], dummy.targetJoints.a[1], dummy.targetJoints.a[2],
                                                  dummy.targetJoints.a[3], dummy.targetJoints.a[4], dummy.targetJoints.a[5]);
            }else if(id == 7){
                dummy.hand->HandAngle = Angle;

            }
        }else if(strstr(&_cmd[3],"[V]") != NULL) {
            int id;
            float speedLimit;
            argNum = sscanf(_cmd.c_str(), "M%d [C] %f", &id,&speedLimit);
            Respond(*usbStreamOutputPtr, "Id:%d V:%.3f\n", id,speedLimit);

            if(id>0 && id<=6){
                dummy.motorJ[id]->SetVelocitySetPoint(speedLimit);
            }
        }
        else if(strstr(&_cmd[3],"[a]") != NULL) {
            int id;
            float Value;
            argNum = sscanf(_cmd.c_str(), "M%d [a] %f", &id,&Value);
            if(argNum == 2 && id >0 && id <= 6 && abs(Value) < 100){
                context->SetJointSpeed(10);
                dummy.targetJoints.a[id-1] += Value;
                context->MoveJ(dummy.targetJoints.a[0], dummy.targetJoints.a[1],
                               dummy.targetJoints.a[2], dummy.targetJoints.a[3],
                               dummy.targetJoints.a[4], dummy.targetJoints.a[5]);
            }

            Respond(*usbStreamOutputPtr, "M%d targetJoint add %.3f\n", id,Value);

        }else if(strstr(&_cmd[3],"[CuLimit]") != NULL) {
            int Id;
            float CuLimit;
            argNum = sscanf(_cmd.c_str(), "M%d [CuLimit] %f", &Id, &CuLimit);
            if (argNum == 2 && Id >0 && Id <= 6 && CuLimit > 0 && CuLimit <= 5) {
                Respond(*usbStreamOutputPtr, "set Id:%d CuLimit:%f A\n", Id, CuLimit);
                dummy.motorJ[Id]->SetCurrentLimit(CuLimit);

            }

        }

    }else if(_cmd[0] == '*'){
        float X,Y,Z,A,B,C,V;
        argNum = sscanf(_cmd.c_str(), "*%f,%f,%f %f,%f,%f %f", &X, &Y, &Z, &A, &B, &C, &V);
        Respond(*usbStreamOutputPtr, "argNum:%d\n",argNum);
        if (argNum == 7) {
            context->SetJointSpeed(V);
            DOF6Kinematic::Pose6D_t tmep;
            dummy.dof6Solver->SolveFK(dummy.targetJoints, tmep);
            tmep.X *= 1000; // m -> mm
            tmep.Y *= 1000; // m -> mm
            tmep.Z *= 1000; // m -> mm
            CommandRightFlag = context->MoveL(tmep.X+X,tmep.Y+Y,tmep.Z+Z,tmep.A+A,tmep.B+B,tmep.C+C);
            if (CommandRightFlag == true) {
                Respond(*usbStreamOutputPtr, "ok,%.3f,%.3f,%.3f %.3f,%.3f,%.3f\n",tmep.X+X,tmep.Y+Y,tmep.Z+Z,tmep.A+A,tmep.B+B,tmep.C+C);
            } else {
                Respond(*usbStreamOutputPtr, "6dof pose err!\n");
            }

        }else {
            Respond(*usbStreamOutputPtr, "err!argNum:%d\n",argNum);

        }

    }

    switch (context->commandMode) {
        case COMMAND_TARGET_POINT_SEQUENTIAL:
        case COMMAND_CONTINUES_TRAJECTORY:
            if (_cmd[0] == '>' || _cmd[0] == '&') {
                float joints[6];
                float speed;

				if(_cmd[0] == '>')
                	argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
                                	joints + 3, joints + 4, joints + 5, &speed);
				else if(_cmd[0] == '&')
					argNum = sscanf(_cmd.c_str(), "&%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
                                	joints + 3, joints + 4, joints + 5, &speed);

               /* if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS){
                    for (int i = 0; i < 6; ++i) {
                        joints[i] = joints[i] / (float) dummy.motorJ[i+1]->reduction * 360;
                    }

                }*/

                if (argNum == 6) {
                    CommandRightFlag = context->MoveJ(joints[0], joints[1], joints[2],
                                   joints[3], joints[4], joints[5]);
                } else if (argNum == 7) {
                    context->SetJointSpeed(speed);
                    CommandRightFlag = context->MoveJ(joints[0], joints[1], joints[2],
                                   joints[3], joints[4], joints[5]);
                }
                // Trigger a transmission immediately, in case IsMoving() returns false
                context->MoveJoints(context->targetJoints);

                while (context->IsMoving() && context->IsEnabled())
                    osDelay(5);
                Respond(*usbStreamOutputPtr, "ok");
                Respond(*uart4StreamOutputPtr, "ok");
            } else if (_cmd[0] == '@') {
                float pose[6];
                float speed;

                argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
                                pose + 3, pose + 4, pose + 5, &speed);
                if (argNum == 6) {
                    CommandRightFlag = context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
                } else if (argNum == 7) {
                    context->SetJointSpeed(speed);
                    CommandRightFlag = context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
                }
                // Trigger a transmission immediately, in case IsMoving() returns false
                context->MoveJoints(context->targetJoints);

                while (context->IsMoving())
                    osDelay(5);
                if (CommandRightFlag == true) {
                    Respond(*usbStreamOutputPtr, "ok");
                    Respond(*uart4StreamOutputPtr, "ok");
                } else {
                    Respond(*usbStreamOutputPtr, "6dof pose err!\n");
                }
            }

            break;

        case COMMAND_TARGET_POINT_INTERRUPTABLE:
            if (_cmd[0] == '>' || _cmd[0] == '&') {
                float joints[6];
                float speed;

				if(_cmd[0] == '>')
                	argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
                                	joints + 3, joints + 4, joints + 5, &speed);
				else if(_cmd[0] == '&')
					argNum = sscanf(_cmd.c_str(), "&%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
                                	joints + 3, joints + 4, joints + 5, &speed);

             /*   if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS){
                    for (int i = 0; i < 6; ++i) {
                        joints[i] = joints[i] / (float) dummy.motorJ[i+1]->reduction * 360;
                    }

                }*/

                if (argNum == 6) {
                    CommandRightFlag = context->MoveJ(joints[0], joints[1], joints[2],
                                   joints[3], joints[4], joints[5]);
                } else if (argNum == 7) {
                    context->SetJointSpeed(speed);
                    CommandRightFlag = context->MoveJ(joints[0], joints[1], joints[2],
                                   joints[3], joints[4], joints[5]);
                }
                dummy.currentJoints = dummy.targetJoints;

                Respond(*usbStreamOutputPtr, "j0:%.3f res:%d\n",joints[0],CommandRightFlag);
                Respond(*uart4StreamOutputPtr, "ok");
            } else if (_cmd[0] == '@') {
                float pose[6];
                float speed;

                argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
                                pose + 3, pose + 4, pose + 5, &speed);
                if (argNum == 6) {
                    CommandRightFlag = context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
                } else if (argNum == 7) {
                    context->SetJointSpeed(speed);
                    CommandRightFlag = context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
                }
                if (CommandRightFlag == true) {
                    Respond(*usbStreamOutputPtr, "ok");
                    Respond(*uart4StreamOutputPtr, "ok");
                } else {
                    Respond(*usbStreamOutputPtr, "6dof pose err!\n");
                }
            }
            break;

        case COMMAND_MOTOR_TUNING:
            break;
    }

    return osMessageQueueGetSpace(commandFifo);
}


void DummyRobot::CommandHandler::ClearFifo() {
    osMessageQueueReset(commandFifo);
}


void DummyRobot::TuningHelper::SetTuningFlag(uint8_t _flag) {
    tuningFlag = _flag;
}


void DummyRobot::TuningHelper::Tick(uint32_t _timeMillis) {
    time += PI * 2 * frequency * (float) _timeMillis / 1000.0f;
    float delta = amplitude * sinf(time);

    for (int i = 1; i <= 6; i++)
        if (tuningFlag & (1 << (i - 1)))
            context->motorJ[i]->SetAngle(delta);
}


void DummyRobot::TuningHelper::SetFreqAndAmp(float _freq, float _amp) {
    if (_freq > 5)_freq = 5;
    else if (_freq < 0.1) _freq = 0.1;
    if (_amp > 50)_amp = 50;
    else if (_amp < 1) _amp = 1;

    frequency = _freq;
    amplitude = _amp;
}
